#include "ctrl_control.h"
#include "ctrl_pid.h"
//Dof_PID_Sample_t Dof_Alt_Smp = {0};
Dof_PID_t SysPID[Dof_Num] = {0};

static void Dof_PID_Init_Similar_Parameter(Dof_PID_t* pDof_PID, float Out_Min, float Out_Max, float K_PosErr_To_SpdSet);

#define sample_cache_max      3
#define PIT_ROL_ERR_AbsMAX    80
#define I_ACT_ERR_AbsMAX      10

#define ANGLE_OUT_MAX         200
#define ANGLE_OUT_MIN         (-200)
#define ALT_OUT_MAX           150
#define ALT_OUT_MIN           (-150)

#define ANGLE_OUT_I_MAX     0.5*ANGLE_OUT_MAX
#define ANGLE_OUT_I_MIN     0.5*ANGLE_OUT_MIN
#define ALT_OUT_I_MAX       0.5*ALT_OUT_MAX
#define ALT_OUT_I_MIN       0.5*ALT_OUT_MIN

void PID_Update_Sample(Dof_PID_t* pPID, float Sample_NewRaw, YesNo_t Sample_Valid)
{
    uint8_t i;
    for(i=SAMPLE_CACHE_MAX-1; i>0; i--)
        pPID->sample_cache[i] = pPID->sample_cache[i-1];
    
    pPID->sample_cache[0] = Sample_NewRaw;
    
    if(Yes == Sample_Valid)
    {
        pPID->sample_cache_cnt++;

        if(pPID->sample_cache_cnt > sample_cache_max)
            pPID->sample_cache_cnt = sample_cache_max;
        pPID->sample = DataFilter(pPID->sample_cache, pPID->sample_cache_cnt);

    }    
    else
    {
        pPID->sample_cache_cnt = 0;
    }
}



/*
void SysCtrl_Update_PID_Error(void)
{
    PID_Update_Error(&SysPID[Rol].PID[Pos], Yes);
    PID_Update_Error(&SysPID[Rol].PID[Spd], Yes);
        
    PID_Update_Error(&SysPID[Pit].PID[Pos], Yes);
    PID_Update_Error(&SysPID[Pit].PID[Spd], Yes);
        
    PID_Update_Error(&SysPID[Yaw].PID[Pos], Yes);
    PID_Update_Error(&SysPID[Yaw].PID[Spd], Yes);

}
*/

void SysCtrl_Update_PID_Calculate(void)
{
    #define LOST_OUT_SCALE      (0.4)
    
    Dof_PID_Update_Calculate(&SysPID[Rol]);
    Dof_PID_Update_Calculate(&SysPID[Pit]);
    Dof_PID_Update_Calculate(&SysPID[Alt]);
    Dof_PID_Update_Calculate(&SysPID[Yaw]);
    
}

int16 Dof_PID_Update_Calculate(Dof_PID_t* pDof_PID)
{

	pDof_PID->Out_Float = PID_Update_Out(pDof_PID);

    if(pDof_PID->Out_Float < ANGLE_OUT_MIN)
    {
        pDof_PID->Out_Float = ANGLE_OUT_MIN;
    }
    else if(pDof_PID->Out_Float > ANGLE_OUT_MAX)
    {
        pDof_PID->Out_Float = ANGLE_OUT_MAX;
    }
    
    pDof_PID->Out_Int16 = (int16)pDof_PID->Out_Float;
    
    return pDof_PID->Out_Int16;
}

 void PID_Update_Error(Dof_PID_t* pPID, YesNo_t Sample_Valid)
{
//	pPID->valid_last=Yes;
    pPID->err_last = pPID->err;
    if(Yes == Sample_Valid)
    {
        pPID->err = pPID->set - pPID->sample;

/*        if((pPID->err) > 0)
            pPID->err_abs = pPID->err;
        else
            pPID->err_p = pPID->err;*/
        
        if(Abs(pPID->err) < I_ACT_ERR_AbsMAX&&Fly_Status_t.Height>60)
        {
            pPID->err_i += pPID->err;
            Constraint_By_Abs(&pPID->err_i, PIT_ROL_ERR_AbsMAX);
        }
        else
            pPID->err_i = 0;
        
      //  if(Yes == pPID->valid_last)
        pPID->err_d = pPID->err - pPID->err_last;
      //  else
      //      pPID->err_d = 0;

    }
    else
    {
        pPID->err = 0;
        pPID->err = 0;
        pPID->err_i = 0;
        pPID->err_d = 0;

    }
}

float PID_Update_Out(Dof_PID_t* pPID)
{
/*    pPID->out_p = pPID->p * pPID->err;
    pPID->out_d = pPID->d * pPID->err_d;
    pPID->out_spd=pPID->dd * pPID->err_spd;*/
	pPID->out_i = pPID->i * pPID->err_i;
	
    if(pPID->out_i < ANGLE_OUT_I_MIN)
        pPID->out_i = ANGLE_OUT_I_MIN;
    else if(pPID->out_i > ANGLE_OUT_I_MAX)
        pPID->out_i = ANGLE_OUT_I_MAX;
    
    pPID->out = pPID->p * pPID->err + pPID->out_i + pPID->d * pPID->err_d ;
    
    return pPID->out;
}

 void PID_Parameter_Init(void)
{
    Dof_PID_Init_Similar_Parameter(&SysPID[Rol], -200, 200, 40.0);
    Dof_PID_Init_Similar_Parameter(&SysPID[Pit], -200, 200, 40.0);
    Dof_PID_Init_Similar_Parameter(&SysPID[Alt], -150, 150, 12.0);
    Dof_PID_Init_Similar_Parameter(&SysPID[Yaw], -200, 200, 10.0);
    
    SysPID[Rol].p = -1.00;
    SysPID[Rol].i = -0.12;
    SysPID[Rol].d = -2.00;
  //  SysPID[Rol].PID[Pos].sample_cache_max = 2;
 //   SysPID[Rol].set = 120 / 2;
  // SysPID[Rol].PID[Pos].set_min = (120 / 2) - (100 / 2);
  //  SysPID[Rol].PID[Pos].set_max = (120 / 2) + (100 / 2);
  //  SysPID[Rol].PID[Pos].err_i_abs_max = 80;
 //   SysPID[Rol].PID[Pos].i_act_err_abs_max = 10;
    
//    SysPID[Rol].PID[Spd].p = -32.0;
//    SysPID[Rol].PID[Spd].i = -1.00;
//    SysPID[Rol].PID[Spd].d = -20.0;
//    SysPID[Rol].PID[Spd].sample_cache_max = 5;
//    SysPID[Rol].PID[Spd].set = 0;
//    SysPID[Rol].PID[Spd].set_min = -4;
//    SysPID[Rol].PID[Spd].set_max = 4;
//    SysPID[Rol].PID[Spd].err_i_abs_max = 30;
//    SysPID[Rol].PID[Spd].i_act_err_abs_max = 10;
    
    SysPID[Pit].p =                    SysPID[Rol].p;
    SysPID[Pit].i =                    SysPID[Rol].i;
    SysPID[Pit].d =                    SysPID[Rol].d;
 //   SysPID[Pit].PID[Pos].sample_cache_max =     SysPID[Rol].PID[Pos].sample_cache_max;
 //   SysPID[Pit].PID[Pos].set =                  100 / 2;
 //   SysPID[Pit].PID[Pos].set_min =              0;
 //   SysPID[Pit].PID[Pos].set_max =              100;
 //   SysPID[Pit].PID[Pos].err_i_abs_max =        SysPID[Rol].PID[Pos].err_i_abs_max;
//    SysPID[Pit].PID[Pos].i_act_err_abs_max =    SysPID[Rol].PID[Pos].i_act_err_abs_max;
    
//    SysPID[Pit].PID[Spd].p =                    SysPID[Rol].PID[Spd].p;
//    SysPID[Pit].PID[Spd].i =                    SysPID[Rol].PID[Spd].i;
//    SysPID[Pit].PID[Spd].d =                    SysPID[Rol].PID[Spd].d;
//    SysPID[Pit].PID[Spd].sample_cache_max =     SysPID[Rol].PID[Spd].sample_cache_max;    
//    SysPID[Pit].PID[Spd].set =                  SysPID[Rol].PID[Spd].set;
//    SysPID[Pit].PID[Spd].set_min =              SysPID[Rol].PID[Spd].set_min;
//    SysPID[Pit].PID[Spd].set_max =              SysPID[Rol].PID[Spd].set_max;
//    SysPID[Pit].PID[Spd].err_i_abs_max =        SysPID[Rol].PID[Spd].err_i_abs_max;
//    SysPID[Pit].PID[Spd].i_act_err_abs_max =    SysPID[Rol].PID[Spd].i_act_err_abs_max;
    
//    SysPID[Alt].PID[Pos].p = 1.20;
//    SysPID[Alt].PID[Pos].i = 0.50;
//    SysPID[Alt].PID[Pos].d = 0.00;
//    SysPID[Alt].PID[Pos].sample_cache_max =  SAMPLE_CACHE_MAX;
//    SysPID[Alt].PID[Pos].set = 80;
//    SysPID[Alt].PID[Pos].set_min = 10;
//    SysPID[Alt].PID[Pos].set_max = 120;
//    SysPID[Alt].PID[Pos].err_i_abs_max = 8;
//    SysPID[Alt].PID[Pos].i_act_err_abs_max = 3;
//    
//    SysPID[Alt].PID[Spd].p = 45.0;
//    SysPID[Alt].PID[Spd].i = 0.00;
//    SysPID[Alt].PID[Spd].d = 0.00;
//    SysPID[Alt].PID[Spd].sample_cache_max =  SAMPLE_CACHE_MAX;
//    SysPID[Alt].PID[Spd].set = 0;
//    SysPID[Alt].PID[Spd].set_min = -2.0;
//    SysPID[Alt].PID[Spd].set_max = 2.0;
//    SysPID[Alt].PID[Spd].err_i_abs_max = 200;
//    SysPID[Alt].PID[Spd].i_act_err_abs_max = 200;
    
    SysPID[Yaw].p = -1.00;
    SysPID[Yaw].i = -0.20;
    SysPID[Yaw].d = -0.00;
//    SysPID[Yaw].PID[Pos].sample_cache_max = 3;
    SysPID[Yaw].set = 0;
//    SysPID[Yaw].PID[Pos].set_min = -800;
//    SysPID[Yaw].PID[Pos].set_max = 800;
//    SysPID[Yaw].PID[Pos].err_i_abs_max = 100;
//   SysPID[Yaw].PID[Pos].i_act_err_abs_max = 100;
    
//    SysPID[Yaw].PID[Spd].p = 0.00;
//    SysPID[Yaw].PID[Spd].i = 0.00;
//    SysPID[Yaw].PID[Spd].d = 0.00;
//    SysPID[Yaw].PID[Spd].sample_cache_max = 3;
//    SysPID[Yaw].PID[Spd].set = 0;
//    SysPID[Yaw].PID[Spd].set_min = -1;
//    SysPID[Yaw].PID[Spd].set_max = 1;
//    SysPID[Yaw].PID[Spd].err_i_abs_max = 5;
//    SysPID[Yaw].PID[Spd].i_act_err_abs_max = 1;
}

static void Dof_PID_Init_Similar_Parameter(Dof_PID_t* pDof_PID, float Out_Min, float Out_Max, float K_PosErr_To_SpdSet)
{
    pDof_PID->Out_Float = 0;
    pDof_PID->Out_Int16 = 0;
/*    pDof_PID->Out_Min = Out_Min;
    pDof_PID->Out_Max = Out_Max;
    pDof_PID->K_PosErr_To_SpdSet = K_PosErr_To_SpdSet;*/
    
//    pDof_PID->PID[Pos].valid_last = Yes;
    Fill_Mem((uint8*)pDof_PID->sample_cache, sizeof(float)*SAMPLE_CACHE_MAX, 0);
    pDof_PID->sample_cache_cnt = 0;
    pDof_PID->err = 0;
    pDof_PID->err_last = 0;
    pDof_PID->err = 0;
    pDof_PID->err_i = 0;
    pDof_PID->err_d = 0;
    pDof_PID->out = 0;
    pDof_PID->out_p = 0;
    pDof_PID->out_i = 0;
//    pDof_PID->PID[Pos].out_i_min = pDof_PID->Out_Min * 0.5;
//    pDof_PID->PID[Pos].out_i_max = pDof_PID->Out_Max * 0.5;
    pDof_PID->out_d = 0;
}


